#!/usr/bin/env python3
# Software License Agreement (BSD License)
#
# Copyright (C) 2012, Jack O'Quin
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the author nor of other contributors may be
#    used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$

"""
Create rviz markers for geographic information maps from Open Street
Map server.
"""


PKG_NAME = "osm_cartography"
import roslib

roslib.load_manifest(PKG_NAME)
import rospy

import sys
import itertools
import geodesy.props
import geodesy.utm
import geodesy.wu_point
from geodesy import bounding_box

from geographic_msgs.msg import GeoPoint
from geographic_msgs.srv import GetGeographicMap
from geometry_msgs.msg import Point
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Vector3
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray

# dynamic parameter reconfiguration
from dynamic_reconfigure.server import Server as ReconfigureServer
import osm_cartography.cfg.VizOSMConfig as Config


class VizNode:
    def __init__(self):
        """ROS node to publish visualization markers for a GeographicMap."""
        rospy.init_node("viz_osm")
        self.config = None

        # advertise visualization marker topic
        self.pub = rospy.Publisher(
            "visualization_marker_array", MarkerArray, latch=True, queue_size=10
        )
        self.map = None
        self.msg = None
        rospy.wait_for_service("get_geographic_map")
        self.get_map = rospy.ServiceProxy("get_geographic_map", GetGeographicMap)

        # refresh the markers every three seconds, making them last four.
        self.timer_interval = rospy.Duration(3)
        self.marker_life = self.timer_interval + rospy.Duration(1)
        rospy.Timer(self.timer_interval, self.timer_callback)

        # register dynamic reconfigure callback, which runs immediately
        self.reconf_server = ReconfigureServer(Config, self.reconfigure)

    def get_markers(self, gmap):
        """Get markers for a GeographicMap message.

        :post: self.msg = visualization markers message
        """
        self.map = gmap
        self.map_points = geodesy.wu_point.WuPointSet(gmap.points)
        self.msg = MarkerArray()
        self.mark_boundaries(ColorRGBA(r=0.5, g=0.5, b=0.5, a=0.8))
        self.mark_way_points(ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.8))

        # define arguments for displaying various feature types
        road_props = set(["bridge", "highway", "tunnel"])
        fargs = [
            (
                lambda f: geodesy.props.match(f, road_props),
                ColorRGBA(r=8.0, g=0.2, b=0.2, a=0.8),
                "roads_osm",
            ),
            (
                lambda f: geodesy.props.match(f, set(["building"])),
                ColorRGBA(r=0.0, g=0.3, b=0.7, a=0.8),
                "buildings_osm",
            ),
            (
                lambda f: geodesy.props.match(f, set(["railway"])),
                ColorRGBA(r=0.0, g=0.7, b=0.7, a=0.8),
                "railroad_osm",
            ),
            (
                lambda f: geodesy.props.match(f, set(["amenity", "landuse"])),
                ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.5),
                "other_osm",
            ),
        ]
        for args in fargs:
            self.mark_features(*args)

    def mark_boundaries(self, color):
        # draw outline of map boundaries
        marker = Marker(
            header=self.map.header,
            ns="bounds_osm",
            id=0,
            type=Marker.LINE_STRIP,
            action=Marker.ADD,
            scale=Vector3(x=2.0),
            color=color,
            lifetime=self.marker_life,
        )

        # Convert bounds latitudes and longitudes to UTM (no
        # altitude), convert UTM points to geometry_msgs/Point
        bbox = self.map.bounds
        min_lat, min_lon, max_lat, max_lon = bounding_box.getLatLong(bbox)
        p0 = geodesy.utm.fromLatLong(min_lat, min_lon).toPoint()
        p1 = geodesy.utm.fromLatLong(min_lat, max_lon).toPoint()
        p2 = geodesy.utm.fromLatLong(max_lat, max_lon).toPoint()
        p3 = geodesy.utm.fromLatLong(max_lat, min_lon).toPoint()

        # add line strips to bounds marker
        marker.points.append(p0)
        marker.points.append(p1)
        marker.points.append(p1)
        marker.points.append(p2)
        marker.points.append(p2)
        marker.points.append(p3)
        marker.points.append(p3)
        marker.points.append(p0)
        self.msg.markers.append(marker)

    def mark_features(self, predicate, color, namespace):
        """Create outline for map features

        :param predicate: function to match desired features
        :param color: RGBA value
        :param namespace: Rviz namespace.

        :todo: differentiate properties for: highway, building,
               bridge, tunnel, amenity, etc.
        """
        index = 0
        # for feature in itertools.ifilter(predicate, self.map.features):
        for feature in filter(predicate, self.map.features):
            marker = Marker(
                header=self.map.header,
                ns=namespace,
                id=index,
                type=Marker.LINE_STRIP,
                action=Marker.ADD,
                scale=Vector3(x=2.0),
                color=color,
                lifetime=self.marker_life,
            )
            index += 1
            prev_point = None
            for mbr in feature.components:
                wu_point = self.map_points.get(mbr.uuid)
                if wu_point:  # this component is a way point
                    p = wu_point.toPointXY()
                    if prev_point:
                        marker.points.append(prev_point)
                        marker.points.append(p)
                    prev_point = p
            self.msg.markers.append(marker)

    def mark_way_points(self, color):
        """Create slightly transparent disks for way-points.

        :param color: disk RGBA value
        """
        index = 0
        for wp in self.map_points:
            marker = Marker(
                header=self.map.header,
                ns="waypoints_osm",
                id=index,
                type=Marker.CYLINDER,
                action=Marker.ADD,
                scale=Vector3(x=2.0, y=2.0, z=0.2),
                color=color,
                lifetime=self.marker_life,
            )
            index += 1
            # use easting and northing coordinates (ignoring altitude)
            marker.pose.position = wp.toPointXY()
            marker.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
            self.msg.markers.append(marker)

    def reconfigure(self, config, level):
        """Dynamic reconfigure callback.

        :param config: New configuration.
        :param level:  0x00000001 bit set if URL changed (ignored).

        :returns: New config if valid, old one otherwise. That updates
                  the dynamic reconfigure GUI window.
        """
        if self.config is None:
            self.config = config
        rospy.loginfo("Map URL: " + str(config["map_url"]))

        try:
            resp = self.get_map(config["map_url"], bounding_box.makeGlobal())
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed:", str(e))
            # ignore new config, it failed
        else:  # get_map returned
            if resp.success:
                self.get_markers(resp.map)
                self.config = config  # save new URL
                # publish visualization markers (on a latched topic)
                self.pub.publish(self.msg)
            else:
                print("get_geographic_map failed, status:", str(resp.status))

        return self.config

    def timer_callback(self, event):
        """ Called periodically to refresh map visualization. """
        if self.msg is not None:
            now = rospy.Time()
            for m in self.msg.markers:
                m.header.stamp = now
            self.pub.publish(self.msg)


def main():
    viznode = VizNode()
    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        pass


if __name__ == "__main__":
    # run main function and exit
    sys.exit(main())
